#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <fstream>

#include <time.h>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "port_comm.h"

int initCamera(int camera);
int startSending();
cv::Mat toGrey(cv::Mat mat);
void printUsage();

void *update(void *arg);

cv::VideoCapture capture;
int main_socket;
sockaddr_in remote_info;

int quality = 0;
int fps = 3;
bool grey = false;
bool sending = true;
pthread_mutex_t mutex;

int previousCamera;


int main(int argc, char *argv[])
{
    pthread_mutex_init(&mutex,NULL);

    // get port to run on, and IP+port to communicate with from command line
    if(argc != 4 && argc != 5)
    {
        printUsage();
        return 1;
    }
    int port_number = atol(argv[1]);
    int remote_port = atol(argv[2]);
    char * remote = argv[3];

    // Step 1 Look up server to get numeric IP address
    hostent * record = gethostbyname(remote);
    if(record==NULL)
    {
        herror("gethostbyname failed");
        exit(1);
    }
    in_addr * addressptr = (in_addr *) record->h_addr;

    // Step 2 Create a socket
    main_socket = socket(AF_INET, SOCK_DGRAM, 0);
    if(main_socket<0)
    {
        perror("socket creation");
        exit(1);
    }

    // Step 3 Create a sockaddr_in to describe the local port
    sockaddr_in local_info;
    // local_info.sin_len = sizeof(local_info);
    local_info.sin_family = AF_INET;
    local_info.sin_addr.s_addr = htonl(INADDR_ANY);
    local_info.sin_port = htons(port_number);

    // Step 4 Bind the socket to the port
    int r = bind(main_socket, (sockaddr *) &local_info, sizeof(local_info));
    if(r<0)
    {
        perror("bind");
        exit(1);
    }
    printf("ready\n");

    // Step 5 Create a sockaddr_in to describe the remote application
    // remote_info.sin_len = sizeof(remote_info);
    remote_info.sin_family = AF_INET;
    remote_info.sin_addr = *addressptr;
    remote_info.sin_port = htons(remote_port);
  
    /*// Register signal handler
    if(signal(SIGINT, signal_handler) == SIG_ERR){
        fprintf(stderr, "Unable to bind handler.\n");
        return 1;
    }*/

    int camera = 0;
    if(argc == 5)
    {
        camera = atoi(argv[4]);
    }
    previousCamera = camera;
    if(initCamera(camera) != 0)
    {
        fprintf(stderr, "Unable to init camera.\n");
    }
    printf("Init Camera complete.\n");

    pthread_t thread;
    pthread_create(&thread,NULL,update,NULL);

    printf("Starting sending.\n");
    startSending();
  
    return 0;
}


int initCamera(int camera)
{
    pthread_mutex_lock(&mutex);
    printf("releasing camera\n");
    if(capture.isOpened())
    {
        capture.release();
    }

    //Init camera
    capture = cv::VideoCapture(camera);
    if(!capture.isOpened()) {
        fprintf(stderr, "Unable to init webcam.\n");
        pthread_mutex_unlock(&mutex);
        return 1;
    }

    //Init camera succesful
    pthread_mutex_unlock(&mutex);
    return 0;
}

int startSending()
{
    cv::vector<int> params;
    params.push_back(CV_IMWRITE_JPEG_QUALITY);
    params.push_back(0); // Set Quality
    
    int frameCount = 0;
   
    cv::Mat frame;
    char message[100000];
    while(true)
    {
        if(frameCount % 10 == 0)
        {
	    usleep(1000000.0 / fps);
            int r = sendto(main_socket, message, 1, MSG_EOR, (sockaddr *) &remote_info, sizeof(remote_info));
            if(r < 0)
            {
                perror("sendto");
            }
        }
        frameCount++;

        if(!sending)
        {
	    usleep(1000000.0 / fps);
            int r = sendto(main_socket, message, 1, MSG_EOR, (sockaddr *) &remote_info, sizeof(remote_info));
            if(r < 0)
            {
                perror("sendto");
            }
            continue;
        }

	usleep(1000000.0 / fps);
        /*int size;
        unsigned char *mode;
        commClient->read_port(&mode, &size);
        int quality = *mode % 101;
        bool grey = *mode > 100;
        delete []mode;*/

        //Grab into a Mat
        pthread_mutex_lock(&mutex);
        if(!capture.isOpened() || !capture.read(frame))
        {
            std::cout << "Error Cap\n";
            frame = cv::Mat(640,480,CV_8UC3);
            cv::vector<cv::Mat> plane;
            split(frame,plane);
            for(int n = 0; n < (int)plane.size(); n++)
            {
                for(int r = 0; r < plane[n].rows; r++)
                {
                    for(int c = 0; c < plane[n].cols; c++)
                    {
                        plane[n].at<uchar>(r,c) = 0;
                    }
                }
            }
            merge(plane,frame);
        }
        pthread_mutex_unlock(&mutex);

        if(grey)
        {
            frame = toGrey(frame);
        }
        params[1] = quality;

        //Move to buf to encode to jpg
        cv::vector<uchar> buf;
        if(!imencode("*.jpg", frame, buf, params))
        {
            fprintf(stderr, "Unable to encode image.\n");
            break;
        }

        if((int)buf.size() > 60000)
        {
            printf("image too large");
            frame = cv::Mat(640,480,CV_8UC3);
            cv::vector<cv::Mat> plane;
            split(frame,plane);
            for(int n = 0; n < (int)plane.size(); n++)
            {
                for(int r = 0; r < plane[n].rows; r++)
                {
                    for(int c = 0; c < plane[n].cols; c++)
                    {
                        plane[n].at<uchar>(r,c) = 255;
                    }
                }
            }
            merge(plane,frame);
            if(!imencode("*.jpg", frame, buf, params))
            {
                fprintf(stderr, "Unable to encode image.\n");
                break;
            }
        }

        //Change to char array to send
        for(int i = 0; i < (int)buf.size(); i++)
        {
            message[i] = (char)buf[i];
        }
        int r = sendto(main_socket, message, buf.size(), MSG_EOR, (sockaddr *) &remote_info, sizeof(remote_info));
        if(r < 0)
        {
            perror("sendto");
        }
        printf("Size: %d\tQuality: %d\tfps: %d\n", r, params[1], fps);
    }

    return 0;
}

cv::Mat toGrey(cv::Mat mat)
{
    cv::Mat ans(mat.rows, mat.cols, CV_8UC1);

    cv::vector<cv::Mat> plane;
    split(mat, plane);

    for(int r = 0; r < mat.rows; r++)
    {
        for(int c = 0; c < mat.cols; c++)
        {
            int sum = 0;
            for(int n = 0; n < (int)plane.size(); n++)
            {
                sum += plane[n].at<uchar>(r, c);
            }
            ans.at<uchar>(r, c) = (sum / (int)plane.size());
        }
    }

    return ans;
}

/*void signal_handler(int sig)
{
    printf("Interrupt Caught.\n");
    inter = true;
}*/

void printUsage()
{
    printf("Usage: camera <localport> <remoteport> <host> [<camera>]\n");
    return;
}

void *update(void *arg)
{
    int SIZE = 20;
    char message[SIZE];
    struct sockaddr_in incoming_info;
    unsigned int socklen = sizeof(incoming_info);
    while(true)
    {
        int r = recvfrom(main_socket, &message, SIZE, 0, (sockaddr *) &incoming_info, &socklen);
        if(r < 0)
        {
            perror("recvfrom");
        }
        int camera = ((int *)message)[0];
        std::ofstream fout;
        fout.open("number");
        fout << camera;
        fout.close();
        if(camera != previousCamera)
        {
            previousCamera = camera;
            initCamera(camera);
        }
        quality = ((int *)message)[1];
        grey = ((int *)message)[2] == 1;
        fps = ((int *)message)[3];
        sending = ((int *)message)[4] == 1;
    }
    return NULL;
}

